#include <time.h>
#include <fstream>
#include <iostream>
#include <math.h>
#include <pthread.h>
#include <thread>
#include <chrono>
#include <numeric>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

// ros头文件
#include <ros/ros.h>
#include <yaml-cpp/yaml.h>
#include <ros/package.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>

// opencv头文件
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/aruco.hpp>
#include <opencv2/aruco/dictionary.hpp>
#include <opencv2/aruco/charuco.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>

// topic 头文件
#include <geometry_msgs/Point.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Bool.h>


// 旋转矩阵msg
#include "gimbal_control/rotation_matrix.h"

#include "gimbal_control/printf_utils.h"

using namespace std;
using namespace cv;

// 输入图像
image_transport::Subscriber image_subscriber;
// 输入开关量
ros::Subscriber switch_subscriber;

// 发布识别后的图像
image_transport::Publisher landpad_pub;

//-------------VISION-----------
Mat img;


double calculation_time;

// 相机话题中的图像同步相关变量
int frame_width, frame_height;
cv::Mat cam_image_copy;
boost::shared_mutex mutex_image_callback;
bool image_status = false;
boost::shared_mutex mutex_image_status;

// 接收消息，允许暂停检测
bool is_suspanded = false;
// 定义发布的旋转矩阵msg
gimbal_control::rotation_matrix rotation_matrix_msg;

// 图像接收回调函数，接收web_cam的话题，并将图像保存在cam_image_copy中
void cameraCallback(const sensor_msgs::ImageConstPtr &msg)
{
    cv_bridge::CvImagePtr cam_image;

    // toCvCopy()将ROS图像转为cv图像
    try
    {
        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception &e)
    {
        PCOUT(0, RED, std::string("cv_bridge exception:") + e.what());
        return;
    }

    if (cam_image)
    {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            cam_image_copy = cam_image->image.clone();
        }
        {
            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutex_image_status);
            image_status = true;
        }
        frame_width = cam_image->image.size().width;
        frame_height = cam_image->image.size().height;
    }
    return;
}

// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
    boost::shared_lock<boost::shared_mutex> lock(mutex_image_status);
    return image_status;
}

void switchCallback(const std_msgs::Bool::ConstPtr &msg)
{
    is_suspanded = !(bool)msg->data;
    // cout << "is_suspanded = " << is_suspanded << endl;
}



int main(int argc, char **argv)
{
    ros::init(argc, argv, "aruco_det");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);

    std::string camera_topic, camera_info;
    int uav_id;

    // /gazebo 节点发布的camera_topic
    if (nh.getParam("camera_topic", camera_topic))
    {
        PCOUT(0, GREEN, std::string("camera_topic is ") + camera_topic);
    }
    else
    {
        camera_topic = "/typhoon_h480_0/cgo3_camera/image_raw";
        PCOUT(0, YELLOW, std::string("didn't find parameter camera_topic, and camera_topic set to") + camera_topic);
    }

    // >>>>>>>>>>>>>>>>>>>接收与发布的话题<<<<<<<<<<<<<<<<<<<<


    ros::Publisher is_det_pub = nh.advertise<std_msgs::Bool>("/is_detected", 10);
    ros::Publisher rotation_matrix_pub = nh.advertise<gimbal_control::rotation_matrix>("/rotation_matrix", 10);
    // 接收开关话题
    switch_subscriber = nh.subscribe("/prometheus/switch/landpad_det", 10, switchCallback);

    // 接收图像的话题
    image_subscriber = it.subscribe(camera_topic.c_str(), 1, cameraCallback);
    // 发布ArUco检测结果的话题
    landpad_pub = it.advertise("/prometheus/camera/rgb/image_landpad_det", 1);
    // -----------------------------------------------------

    // 要发布的图像
    sensor_msgs::ImagePtr msg_ellipse;
    std_msgs::Bool dete_msg;
    //std::string ros_path = ros::package::getPath("prometheus_detection");

    // // 读取参数文档camera_param.yaml中的参数值；
    // YAML::Node camera_config = YAML::LoadFile(camera_info);
    // 相机内部参数
    double fx = 205.47;//camera_config["fx"].as<double>();
    double fy = 205.47;//camera_config["fy"].as<double>();
    double cx = 320;//camera_config["x0"].as<double>();
    double cy = 180;//camera_config["y0"].as<double>();


    double landpad_det_len = 0.6; //camera_config["landpad_det_len"].as<double>();
 
    //--------------------------相机参数赋值---------------------
    // 相机内参
    cv::Mat camera_matrix;
    camera_matrix = cv::Mat(3, 3, CV_64FC1, cv::Scalar::all(0));
    camera_matrix.ptr<double>(0)[0] = fx;
    camera_matrix.ptr<double>(0)[2] = cx;
    camera_matrix.ptr<double>(1)[1] = fy;
    camera_matrix.ptr<double>(1)[2] = cy;
    camera_matrix.ptr<double>(2)[2] = 1.0f;
    // 相机畸变参数k1 k2 p1 p2 k3
    cv::Mat distortion_coefficients;
    distortion_coefficients = cv::Mat(5, 1, CV_64FC1, cv::Scalar::all(0));
    distortion_coefficients.ptr<double>(0)[0] = 0;
    distortion_coefficients.ptr<double>(1)[0] = 0;
    distortion_coefficients.ptr<double>(2)[0] = 0;
    distortion_coefficients.ptr<double>(3)[0] = 0;
    distortion_coefficients.ptr<double>(4)[0] = 0;

    // ArUco Marker字典选择以及旋转向量和评议向量初始化
    Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250);

    // 旋转向量，转移向量
    vector<double> rv(3), tv(3);
    // 旋转向量，转移向量
    cv::Mat rvec(rv), tvec(tv);
    // cv::VideoCapture capture(0);
    // 历史值的记录
    float last_x(0), last_y(0), last_z(0), last_yaw(0), last_az(0), last_ay(0), last_ax(0), 
    last_qx(0), last_qy(0), last_qz(0), last_qw(0);
    float last_ix ,last_iy;
    cv::Mat rotation_matrix, last_rotation_matrix;

    // 切换标志位，是否检测
    bool switch_state = is_suspanded;

    float landpad_det_len_big = 0.5392f;
    float landpad_det_len_small = 0.0529f;
    // 节点运行频率： 20hz 【视觉端解算频率大概为20HZ】
    ros::Rate loopRate(20);

    // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主循环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

    // const auto wait_duration = std::chrono::milliseconds(2000);
    while (ros::ok())
    {
        ros::spinOnce();
        //没收到图像，则输出等候信息
        if (!getImageStatus() && ros::ok())
        {
            PCOUT(-1, WHITE, "Waiting for image");
            continue;
        }

        // 没有暂停检测
        if (true)
        {
            {
                boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
                img = cam_image_copy.clone();
            }

            clock_t start = clock();

            //------------------调用ArUco Marker库对图像进行识别--------------
            // markerids存储每个识别到二维码的编号
            std::vector<int> markerids, markerids_deted;
            // markerCorners每个marker对应的四个角点(顺时针)的像素坐标(x,y)
            vector<vector<Point2f>> markerCorners, markerCorners_deted, rejectedCandidate;

            Ptr<cv::aruco::DetectorParameters> parameters = cv::aruco::DetectorParameters::create();
            cv::aruco::detectMarkers(img, dictionary, markerCorners_deted, markerids_deted, parameters, rejectedCandidate);

            // 过滤检测信息
            // 如果检测到二维码，按顺序进行检测，对应到指定id的储存到markerids中
            // 19中间最大， 43中间最小， 1左上角， 2右上角， 3左下角， 4右下角
            vector<int> landpad_id{19, 43};
            for (auto i = std::begin(landpad_id); i != std::end(landpad_id); ++i)
            {
                for (int j = 0; j < markerids_deted.size(); ++j)
                {
                    if (markerids_deted[j] == *i)
                    {
                        markerCorners.push_back(markerCorners_deted[j]);
                        markerids.push_back(*i);
                        break;
                    }
                }
            }


            // 不同的id有着不同的大小参数
            if (!markerids.empty())
            {
                // 可视化
                aruco::drawDetectedMarkers(img, markerCorners, markerids);

                // 旋转向量和平移向量
                vector<Vec3d> rvecs, tvecs;
                if ((markerids[0] == 19 && markerids[1] == 43) || (markerids[0] == 43 && markerids[1] == 19))
                {
                    for (size_t i = 0; i < markerids.size(); ++i)
                    {
                        std::vector<std::vector<cv::Point2f>> singleMarkerCorners = {markerCorners[i]};
                        std::vector<cv::Vec3d> singleRvecs, singleTvecs;

                        if (markerids[i] == 19)
                        {
                            aruco::estimatePoseSingleMarkers(singleMarkerCorners, landpad_det_len_big, camera_matrix, distortion_coefficients, singleRvecs, singleTvecs);
                        }
                        else if (markerids[i] == 43)
                        {
                            aruco::estimatePoseSingleMarkers(singleMarkerCorners, landpad_det_len_small, camera_matrix, distortion_coefficients, singleRvecs, singleTvecs);
                        }

                        rvecs.push_back(singleRvecs[0]);
                        tvecs.push_back(singleTvecs[0]);
                    }
                }
                else if (markerids[0] == 19)
                {
                    aruco::estimatePoseSingleMarkers(markerCorners, landpad_det_len_big, camera_matrix, distortion_coefficients, rvecs, tvecs);
                }
                else if (markerids[0] == 43)
                {
                    aruco::estimatePoseSingleMarkers(markerCorners, landpad_det_len_small, camera_matrix, distortion_coefficients, rvecs, tvecs);
                }
                for (int i = 0; i < markerids.size(); ++i)
                {
                    aruco::drawAxis(img, camera_matrix, distortion_coefficients, rvecs[i], tvecs[i], landpad_det_len  * 0.5f);
                }


                for(int i = 0; i < 3; i++)
                {
                    rotation_matrix_msg.translation_vector[i] = tvecs[0][i];
                }
                // 相机姿态: 旋转向量 -> 旋转矩阵
                cv::Rodrigues(rvecs[0], rotation_matrix);
                for(int i = 0; i < 3; i++)
                {
                    for(int j = 0; j < 3; j++)
                    {
                        rotation_matrix_msg.rotation_matrix[i*3+j] = rotation_matrix.at<double>(i,j);
                    }
                }
                last_rotation_matrix = rotation_matrix.clone();
                dete_msg.data = true;
                is_det_pub.publish(dete_msg);
                
                rotation_matrix_pub.publish(rotation_matrix_msg);
            }
            else
            {
                dete_msg.data = false;
                is_det_pub.publish(dete_msg);             
            }
            cout << "rotation_matrix :" << endl;
            for(int i = 0; i < 3; i++)
            {
                for(int j = 0; j < 3; j++)
                {
                    cout<< rotation_matrix_msg.rotation_matrix[i*3 + j] << " ";
                }
                cout << endl;
            }
            cout << "translation_vector :" << endl; 
            for(int i = 0; i < 3; i++)
                cout <<  rotation_matrix_msg.translation_vector[i] << " ";
            cout << endl;
           
            // 计算算法运行时间
            clock_t finish = clock();
            calculation_time = (finish - start) / 1000;

            // CV图像转为ROS图像
            msg_ellipse = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img).toImageMsg();
            landpad_pub.publish(msg_ellipse);

            // cv::imshow("test",img);
            // cv::waitKey(1);
        }
        loopRate.sleep();
    }
}
